
## if Misha connected encoders the wrong way set this to True !
#REVERTED_SENSORS = True

if 1:
	try:
		import psyco
		psyco.full()
	except ImportError:
		print 'Psyco not installed, encoder calculations will just run slower' 

import math
from math import sin, cos
import struct

#AXIAL_DISTANCE = 270.0
AXIAL_DISTANCE = 136.0
pi2 = math.pi * 2.0

## ticks * 76.0 * math.pi / 800.0
## ticks * 52.0 * math.pi / 800.0

#SIGNAL_TO_DISTANCE = 0.29845130209103032
#4647 4718    /  1000.0
#3315 3363    /  810.0		810.0 / 3339 # 0.24258760107816711590296495956873
#SIGNAL_TO_DISTANCE = 0.21356113187399893219434063000534 # 1000/ 4682.5
#SIGNAL_TO_DISTANCE = 0.22807436647608302404865279478687
SIGNAL_TO_DISTANCE = 0.2042035224833365605


def calculate(position, left, right, timestamp):

	if left==0 and right==0: return
	left *= SIGNAL_TO_DISTANCE
	right *= SIGNAL_TO_DISTANCE
	
	old_x, old_y = position.x, position.y 

	if left == right:
		position.x += left * cos(position.angle)
		position.y += left * sin(position.angle)
	if left != right:
		position.x += (AXIAL_DISTANCE*(right+left)) / (2.0*(right-left)) * (sin(((right-left)/AXIAL_DISTANCE)+position.angle) - sin(position.angle))
		position.y -= (AXIAL_DISTANCE*(right+left)) / (2.0*(right-left)) * (cos(((right-left)/AXIAL_DISTANCE)+position.angle) - cos(position.angle))
		position.angle += (right-left) / AXIAL_DISTANCE

	if position.angle >= pi2: 
		position.angle -= pi2

	if position.angle < 0:
		position.angle += pi2
	
	## velocity calculation 
	time_diff = timestamp - position.calculation_timestamp
	if time_diff > 0.0:
		position.velocity_x = (position.x - old_x) / time_diff
		position.velocity_y = (position.y - old_y) / time_diff
	
	position.calculation_timestamp = timestamp 

PACKET_LENGTH = 5
STARTER = '@'

class EncoderData:
	def __init__(self, left, right, time_stamp):
		self.left = left
		self.right = right
		self.ts = time_stamp
	
	def __str__(self):
		return '[%s] [%s]' % (self.left, self.right, )

left_, right_ = 0, 0
def ISR( comm_data, new_chunk, time_stamp ):
	global left_
	global right_
	
	comm_data.chunk += new_chunk
	while len(comm_data.chunk) >= PACKET_LENGTH:
		if comm_data.chunk[0] == STARTER:
			
			left, right = struct.unpack('<hh', comm_data.chunk[1:PACKET_LENGTH])
			right = -right 
			
			left_ += left
			right_ += right
			#print left_, right_
			
			record = EncoderData(left, right, time_stamp)
			comm_data.chunk = comm_data.chunk[PACKET_LENGTH:]
			## got information
			comm_data.history.append(record)
		else:
			comm_data.chunk = comm_data.chunk[1:]

if __name__ == '__main__':
	from encoder import Position
	print 'calculation test ..'
	l = 15.707963267948966192313216916398
	r = 439.82297150257105338477007365913
	pos = Position()
	
	for x in range(4):
		calculate(pos, l, r)
		print 'x: %s y: %s a: %s' % ()




